import lejos.navigation.TachoNavigator;
import lejos.nxt.LightSensor;
import lejos.nxt.SensorPort;
import lejos.nxt.TouchSensor;
import lejos.nxt.UltrasonicSensor;
import lejos.nxt.comm.RConsole;
import lejos.subsumption.Arbitrator;
import lejos.subsumption.Behavior;

//public class Wall implements SensorPortListener {
public class Test {
	private boolean[] hitWallLR = { false, false };
	private TachoNavigator tn;
	private TouchSensor tsLeft;
	private TouchSensor tsRight;
	private UltrasonicSensor us;
	private Arbitrator arby;

	public Test(TachoNavigator tn, TouchSensor tsLeft, TouchSensor tsRight, UltrasonicSensor us) {
		this.tn = tn;
		this.tsLeft = tsLeft;
		this.tsRight = tsRight;
		this.us = us;
		
//		SensorPort.S3.addSensorPortListener(this);
//		SensorPort.S4.addSensorPortListener(this);
		State state = new State(us, new LightSensor(SensorPort.S1), SensorPort.S1);
		Behavior drive = new Drive(tn, state);
		Behavior sound = new JukeBox();
		Behavior[] bArray = { drive, sound };
		Arbitrator arby = new Arbitrator(bArray);
		arby.start();
		
	}
	
	public void start() {
		arby.start();
	}

	private boolean isWallDistanceLow() {
		return us.getDistance() < 10;	
	}
	
	private boolean isWallDistanceHigh() {
		return us.getDistance() >= 10;	
	}		
	
	/**
	 * Nach vorne fahren
	 */
	Behavior driveForward = new Behavior() {		
		private boolean suppress = false; 
		
		public void action() {
			RConsole.println("==== START: lostRightWall.action()");
			
			tn.forward();
			
			RConsole.println("==== ENDE: lostRightWall.action()");
		}

		public void suppress() {
			RConsole.println("==== START: lostRightWall.suppress()");
			
			//tn.stop();
			
			RConsole.println("==== ENDE: lostRightWall.suppress()");
		}

		public boolean takeControl() {
			RConsole.println("-- RUN: lostRightWall.takeControl()");			
			RConsole.println("us.getDistance() = " + us.getDistance());
			RConsole.println("suppress = " + suppress);
			RConsole.println("");
			
			return isWallDistanceHigh();
		}
	};

	/**
	 * Mauer gefunden
	 */
	Behavior foundRightWall = new Behavior() {		
		private boolean suppress = false;
		
		public void action() {
			RConsole.println("==== START: foundRightWall.action()");
						
			tn.turn(1000, 30, true);
			
			RConsole.println("==== ENDE: foundRightWall.action()");			
		}

		public void suppress() {
			RConsole.println("==== START: foundRightWall.suppress()");
			
			tn.stop();			
			
			RConsole.println("==== ENDE: foundRightWall.suppress()");
		}

		public boolean takeControl() {
			RConsole.println("-- RUN: foundRightWall.takeControl()");			
			RConsole.println("us.getDistance() = " + us.getDistance());
			RConsole.println("suppress = " + suppress);
			RConsole.println("");
			
			return isWallDistanceLow();
		}
	};

	/**
	 * Mit rechter Mauer kollidiert
	 */	
	Behavior hitRightWall = new Behavior() {		
		public void action() {
			RConsole.println("==== START: hitRightWall.action()");
			
			// Radiusmittelpunkt rechts
			tn.turn(-50, -90);
			//tn.rotate(45);
			
			RConsole.println("==== ENDE: hitRightWall.action()");		
		}

		public void suppress() {
			RConsole.println("==== START: hitRightWall.suppress()");
						
//			tn.stop();
			
			RConsole.println("==== ENDE: hitRightWall.suppress()");
		}

		public boolean takeControl() {
			RConsole.println("-- RUN: hitRightWall.takeControl()");
			RConsole.println("hitWall = " + hitWallLR[0] + "," + hitWallLR[1]);
			RConsole.println("");
			
			return tsRight.isPressed();
		}
	};
	
	/**
	 * Mit linker Mauer kollidiert
	 */	
	Behavior hitLeftWall = new Behavior() {		
		public void action() {
			RConsole.println("==== START: hitLeftWall.action()");
			
			// Radiusmittelpunkt links
			tn.turn(50, -90);
			//tn.rotate(-45);
			
			RConsole.println("==== ENDE: hitLeftWall.action()");		
		}

		public void suppress() {
			RConsole.println("==== START: hitLeftWall.suppress()");
						
//			tn.stop();
			
			RConsole.println("==== ENDE: hitLeftWall.suppress()");
		}

		public boolean takeControl() {
			RConsole.println("-- RUN: hitLeftWall.takeControl()");
			RConsole.println("hitWall = " + hitWallLR[0] + "," + hitWallLR[1]);
			RConsole.println("");
			
			return tsLeft.isPressed();
		}
	};
	
//	public void stateChanged(SensorPort source, int oldValue, int newValue) {
//		// TouchSensor left
//		if (source == SensorPort.S4) {
//			hitWallLR[0] = (tsLeft.isPressed()) ? true : false;
//		}
//		// TouchSensor right
//		if (source == SensorPort.S3) {
//			hitWallLR[1] = (tsRight.isPressed()) ? true : false;
//		}
//	}	
}
